#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :TECS.py
@Description  :
@Time         :2024/05/23 21:32:54
@Author       :Lin Yuheng
@Version      :1.0
'''

import rospy
import time
from typing import List
from tools.mathlib import *
from tools.vector import *
from tools.filter import *
from tools.params import *

class TECSController:
    def __init__(self):
        
        self._tecs_mode = ECL_TECS_MODE_NORMAL

        # timestamps
        self._state_update_timestamp = 0.0  # last timestamp of the 50 Hz function call
        self._speed_update_timestamp = 0.0  # last timestamp of the speed function call
        self._pitch_update_timestamp = 0.0 # last timestamp of the pitch function call

        # controller parameters
        self._hgt_estimate_freq = 3.0       # cross-over frequency of the height rate complementary filter (rad/sec)
        self._tas_estimate_freq = 2.0       # cross-over frequency of the true airspeed complementary filter (rad/sec)
        self._max_climb_rate = 5.0          # climb rate produced by max allowed throtlle (m/sec)
        self._min_sink_rate = 2.0           # sink rate produced by min allowed throttle (m/sec)
        self._max_sink_rate = 5.0           # sink rate produced by max allowed throttle (m/sec)
        self._pitch_time_constant = 5.0     # control time contant used by the pitch demand calculation (sec)
        self._throttle_time_constant = 8.0  # control time contant used by the throttle demand calculation (sec)
        self._pitch_damping_gain = 0.0      # damping gain of the pitch demand calculation (sec)
        self._throttle_damping_gain = 0.5   # damping gain of the throttle demand calculation (sec)
        self._integrator_gain = 0.1         # integrator gain used by the throttle and pitch demand calculation (sec)
        self._vert_accel_limit = 10.0       # magnitude of the maximum vertical acceleration allowed (m/sec**2)
        self._load_factor_correction = 15.0 # gain from normal load factor increase to total energy rate demand (m**2/sec**3)
        self._pitch_speed_weight = 1.0      # speed control weighting used by the pitch demand calculation (rad/sec)
        self._height_error_gain = 0.05      # gain from height error to demand climb rate (1/sec)
        self._height_setpoint_gain_ff = 0.8 # gain from height demand derivate to demand climb rate
        self._speed_error_gain = 0.02       # gain from speed error to demand speed rate (1/sec)
        self._indicated_airspeed_min = 3.0  # equivalent airspeed demand lower limit (m/sec)
        self._indicated_airspeed_max = 30.0 # equivalent airspeed demand upper limit (m/sec)
        self._throttle_slewrate = 0.0       # throttle demand slew rate limit (1/sec)

        # controller outputs
        self._throttle_setpoint = 0.0 # normalized throttle demand (0 to 1)
        self._pitch_setpoint = 0.0  # pitch demand (rad)

        # complimentary filter parameters
        self._vert_accel_state = 0.0 # complimentary filter state - height second derivative (m/sec**2)
        self._vert_vel_state = 0.0   # complimentary filter state - height rate (m/sec)
        self._vert_pos_state = 0.0   # complimentary filter state - height (m)
        self._tas_rate_state = 0.0   # complimentary filter state - true airspeed first derivative (m/sec**2)
        self._tas_state = 0.0        # complimentary filter state - true airspeed (m/sec)

        # controller stated
        self._throttle_integ_state = 0   # throttle integrator state
        self._pitch_integ_state = 0      # pitch integrator state (rad)
        self._last_throttle_setpoint = 0 # throttle demand rate limiter state (1/sec)
        self._last_pitch_setpoint = 0    # pitch demand rate limiter state (rad/sec)
        self._speed_derivative = 0       # rate of change of speed along X axis (m/sec**2)

        # speed demand calculations
        self._EAS_state = 0.0           # equivalent airspeed (m/sec)
        self._TAS_max = 30.0            # true airspeed demand upper limit (m/sec)
        self._TAS_min = 3.0             # true airspeed demand lower limit (m/sec)
        self._TAS_setpoint = 0.0        # current airspeed demand (m/sec)
        self._TAS_setpoint_last = 0.0   # previous airspeed demand (m/sec)
        self._EAS_setpoint = 0.0        # equivalent airspeed demand (m/sec)
        self._TAS_setpoint_adj = 0.0    # airspeed demand tracked by the TECS controller (m/sec)
        self._TAS_rate_setpoint = 0.0   # true airspeed rate demand tracked by the TECS controller (m/sec**2)

        # height demand calculations
        self._hgt_setpoint = 0.0          # demanded height tracked by the TECS algorithm (m)
        self._hgt_setpoint_in_prev = 0.0  # previous value of _hgt_setpoint after noise filtering (m)
        self._hgt_setpoint_prev = 0.0     # previous value of _hgt_setpoint after noise filtering and rate limiting (m)
        self._hgt_setpoint_adj = 0.0      # demanded height used by the control loops after all filtering has been applied (m)
        self._hgt_setpoint_adj_prev = 0.0 # value of _hgt_setpoint_adj from previous frame (m)
        self._hgt_rate_setpoint = 0.0     # demanded climb rate tracked by the TECS algorithm

        # vehicle physical limits
        self._pitch_setpoint_unc = 0.0    # pitch demand unclamping (rad)
        self._STE_rate_max = 0.0          # specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
        self._STE_rate_min = 0.0          # specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
        self._throttle_setpoint_max = 0.0 # normalised throttle upper limit
        self._throttle_setpoint_min = 0.0 # normalised throttle lower limit
        self._pitch_setpoint_max = 0.5    # pitch demand upper limit (rad)
        self._pitch_setpoint_min = -0.5    # pitch demand lower limit (rad)

        # specific energy quantities
        self._SPE_setpoint = 0.0        # specific potential energy demand (m**2/sec**2)
        self._SKE_setpoint = 0.0        # specific kinetic energy demand (m**2/sec**2)
        self._SPE_rate_setpoint = 0.0   # specific potential energy rate demand (m**2/sec**3)
        self._SKE_rate_setpoint = 0.0   # specific kinetic energy rate demand (m**2/sec**3)
        self._SPE_estimate = 0.0        # specific potential energy estimate (m**2/sec**2)
        self._SKE_estimate = 0.0        # specific kinetic energy estimate (m**2/sec**2)
        self._SPE_rate = 0.0            # specific potential energy rate estimate (m**2/sec**3)
        self._SKE_rate = 0.0            # specific kinetic energy rate estimate (m**2/sec**3)

        # specific energy error quantities
        self._STE_error = 0.0       # specific total energy error (m**2/sec**2)
        self._STE_rate_error = 0.0  # specific total energy rate error (m**2/sec**3)
        self._SEB_error = 0.0       # specific energy balance error (m**2/sec**2)
        self._SEB_rate_error = 0.0  # specific energy balance rate error (m**2/sec**3)

        # time step (non-fixed)
        DT_DEFAULT = 0.02     # default value for _dt (sec)
        self._dt = DT_DEFAULT # Time since last update of main TECS loop (sec)

        # controller mode logic
        self._underspeed_detected = False           # true when an underspeed condition has been detected
        self._detect_underspeed_enabled = True      # true when underspeed detection is enabled
        self._uncommanded_descent_recovery = False  # true when a continuous descent caused by an unachievable airspeed demand has been detected
        self._climbout_mode_active = False          # true when the climbout mode is active
        self._airspeed_enabled = False              # true when the airspeed demand is enabled
        self._states_initialized = False            # true when the TECS states have been initialized
        self._in_air = False                        # true when the vehicle is flying

    # Get the current airspeed sensor enabled state
    def airspeed_sensor_enabled(self):
        return self._airspeed_enabled
    
    # Set the airspeed enable state
    def enable_airspeed(self, enabled:bool):
        self._airspeed_enabled = enabled
    
    # Update the following vehicle kinematic state estimates:
    # - Vertical position, velocity and acceleration
    # - Speed derivative
    # Must be called prior to updating tecs control loops
    # Must be called at 50 Hz or greater
    def update_vehicle_state_estimates(
        self,
        airspeed: float,
        rotMat: List[List[float]],
        accel_body: List[float],
        altitude_lock: bool,
        in_air: bool,
        altitude: float,
        vz_valid: bool,
        vz: float,
        az: float,
    ):
        # 计算算自上次更新以来经过的时间
        # now = rospy.Time.now()
        now = time.time()
        dt = constrain(now - self._state_update_timestamp, DT_MIN, DT_MAX)

        reset_altitude = False

        # 初始化
        if self._state_update_timestamp == 0 or dt > DT_MAX:
            dt = DT_DEFAULT
            reset_altitude = True

        if not altitude_lock or not in_air:
            reset_altitude = True

        if reset_altitude:
            self._vert_pos_state = altitude

            if vz_valid:
                self._vert_vel_state = -vz

            else:
                self._vert_vel_state = 0.0

            self._vert_accel_state = 0.0
            self._states_initialized = False

        # 更新时间戳
        self._state_update_timestamp = now
        self._EAS_state = airspeed

        self._in_air = in_air

        # 生成高度和爬升率状态估计
        if vz_valid:
            # Set the velocity and position state to the the INS data
            self._vert_vel_state = -vz
            self._vert_pos_state = altitude
        else:
            # Get height acceleration

            hgt_ddot_mea = -az

            # If we have no vertical INS data, estimate the vertical velocity using a complementary filter
            # Perform filter calculation using backwards Euler integration
            # Coefficients selected to place all three filter poles at omega
            # Reference Paper: Optimising the Gains of the Baro-Inertial Vertical Channel
            # Widnall W.S, Sinha P.K, AIAA Journal of Guidance and Control, 78-1307R
            omega2 = self._hgt_estimate_freq ** 2
            hgt_err = altitude - self._vert_pos_state
            vert_accel_input = hgt_err * omega2 * self._hgt_estimate_freq
            self._vert_accel_state += vert_accel_input * dt
            vert_vel_input = (
                self._vert_accel_state + hgt_ddot_mea + hgt_err * omega2 * 3.0
            )
            self._vert_vel_state += vert_vel_input * dt
            vert_pos_input = (
                self._vert_vel_state + hgt_err * self._hgt_estimate_freq * 3.0
            )

            # If more than 1 second has elapsed since the last update, reset the position state to the measured height
            if reset_altitude:
                self._vert_pos_state = altitude
            else:
                self._vert_pos_state += vert_pos_input * dt

        # Update and average speed rate of change if airspeed is being measured
        if ISFINITE(airspeed) & self.airspeed_sensor_enabled():
            # Assuming the vehicle is flying X axis forward, use the X axis measured acceleration
            # compensated for gravity to estimate the rate of change of speed
            speed_deriv_raw = rotMat[2][0] * GRAVITY + accel_body[0]

            # Apply some noise filtering
            self._speed_derivative = 0.95 * self._speed_derivative + 0.05 * speed_deriv_raw
        else:
            self._speed_derivative = 0.0

        if not self._in_air:
            self._states_initialized = False
    
    def get_throttle_setpoint(self):
        return self._throttle_setpoint
    
    def get_pitch_setpoint(self):
        return self._pitch_setpoint
    
    def get_speed_weight(self):
        return self._pitch_speed_weight
    
    def reset_states(self):
        self._states_initialized = False

    def set_detect_underspeed_enabled(self, enabled:bool):
        self._detect_underspeed_enabled = enabled

    # Setters for controller parameters
    def set_time_const(self, time_const:float):
        self._time_const = time_const

    def set_integrator_gain(self, gain:float):
        self._integrator_gain = gain
    
    def set_min_sink_rate(self, min_sink_rate:float):
        self._min_sink_rate = min_sink_rate
    
    def set_max_sink_rate(self, max_sink_rate:float):
        self._max_sink_rate = max_sink_rate
    
    def set_max_climb_rate(self, climb_rate:float):
        self._max_climb_rate = climb_rate

    def set_height_comp_filter_omega(self, omega:float):
        self._hgt_estimate_freq = omega

    def set_heightrate_ff(self, heightrate_ff:float):
        self._height_setpoint_gain_ff = heightrate_ff

    def set_heightrate_p(self, heightrate_p:float):
        self._height_error_gain = heightrate_p

    def set_indicated_airspeed_max(self, airspeed:float):
        self._indicated_airspeed_max = airspeed
    
    def set_indicated_airspeed_min(self, airspeed:float):
        self._indicated_airspeed_min = airspeed

    def set_pitch_damping(self, damping):
        self._pitch_damping_gain = damping

    def set_vertical_accel_limit(self, limit:float):
        self._vert_accel_limit = limit
    
    def set_speed_comp_filter_omega(self, omega:float):
        self._tas_estimate_freq = omega
    
    def set_speed_weight(self, weight:float):
        self._pitch_speed_weight = weight
    
    def set_speedrate_p(self, speedrate_p:float):
        self._speed_error_gain = speedrate_p

    def set_time_const_throttle(self, time_const_throttle:float):
        self._time_const_throttle = time_const_throttle

    def set_throttle_damp(self, throttle_damp:float):
        self._throttle_damping_gain = throttle_damp
    
    def set_throttle_slewrate(self, slewrate:float):
        self._throttle_slewrate = slewrate

    def set_roll_throttle_compensation(self, compensation:float):
        self._load_factor_correction = compensation
    
    # TECS status
    def timestamp(self):
        return self._pitch_update_timestamp

    def tecs_mode(self):
        return self._tecs_mode
    
    def hgt_setpoint_adj(self):
        return self._hgt_setpoint_adj
    
    def vert_pos_state(self):
        return self._vert_pos_state
    
    def TAS_setpoint_ad(self):
        return self._TAS_setpoint_adj
    
    def tas_state(self):
        return self._tas_state
    
    def hgt_rate_setpoint(self):
        return self.hgt_rate_setpoint
    
    def vert_vel_state(self):
        return self._vert_vel_state
    
    def TAS_rate_setpoint(self):
        return self._TAS_rate_setpoint
    
    def speed_derivative(self):
        return self._speed_derivative
    
    def STE_error(self):
        return self._STE_error
    
    def STE_rate_error(self):
        return self._STE_rate_error
    
    def SEB_error(self):
        return self._SEB_error
    
    def SEB_rate_error(self):
        return self._SEB_rate_error
    
    def throttle_integ_state(self):
        return self._throttle_integ_state
    
    def pitch_integ_state(self):
        return self._pitch_integ_state
    
    # Handle the altitude reset
    # If the estimation system resets the height in on discrete step this
    # will gracefully even out the reset over time
    def handle_alt_step(self, delta_alt:float, altitude:float):
        # add height reset delta to all variables involved in filtering the demanded height
        self._hgt_setpoint_in_prev += delta_alt
        self._hgt_setpoint_prev += delta_alt
        self._hgt_setpoint_adj_prev += delta_alt

        # reset height states
        self._vert_pos_state = altitude
        self._vert_accel_state = 0.0
        self._vert_vel_state = 0.0

    def __update_speed_state(self, airspeed_setpoint:float, indicated_airspeed:float, EAS2TAS:float):

        # Calculate the time in seconds since the last update and use the default time step value if out of bounds
        now = time.time()
        dt = constrain(now - self._speed_update_timestamp, DT_MIN, DT_MAX)

        # Convert equivalent airspeed to true airspeed
        self._EAS_setpoint = airspeed_setpoint
        self._TAS_setpoint = self._EAS_setpoint * EAS2TAS
        self._TAS_max = self._indicated_airspeed_max * EAS2TAS
        self._TAS_min = self._indicated_airspeed_min * EAS2TAS

        # If airspeed measurement are not being used, fix the airspeed estimate to halfway between min and max limits
        if not (ISFINITE(indicated_airspeed) and self.airspeed_sensor_enabled()):
            self._EAS_state = 0.5 * (self._indicated_airspeed_min + self._indicated_airspeed_max)
        else:
            self._EAS_state = indicated_airspeed

        # If first time through or not flying, reset airspeed states
        if (self._speed_update_timestamp == 0.0 or not self._in_air):
            self._tas_rate_state = 0.0
            self._tas_state = self._EAS_state * EAS2TAS

        # 通过一个二阶补偿滤波器来获得一个平滑的空速估计
        # Update TAS rate state
        # 更新空速变化率
        tas_error = (self._EAS_state * EAS2TAS) - self._tas_state
        tas_rate_state_input = tas_error * (self._tas_estimate_freq ** 2)

        # limit integrator input to prevent windup
        if self._tas_state < 3.1:
            tas_rate_state_input = max(tas_rate_state_input, 0)

        # Update TAS state
        # 更新空速估计
        self._tas_rate_state += tas_rate_state_input * dt
        tas_state_input = self._tas_rate_state + self._speed_derivative + tas_error * self._tas_estimate_freq * 1.4142
        self._tas_state += tas_state_input * dt


        # Limit the airspeed state to a minimum of 3m/s
        self._tas_state = max(self._tas_state, 3.0)
        self._speed_update_timestamp = now

    def __update_speed_setpoint(self):
        # Set the airspeed demand to the minimum value if an underspeed
        # or a uncontrolled descent condition exists to maximise climb rate
        if self._uncommanded_descent_recovery or self._underspeed_detected:
            self._TAS_setpoint = self._TAS_min

        self._TAS_setpoint = constrain(self._TAS_setpoint, self._TAS_min, self._TAS_max)

        # Calculate limits for the demanded rate of change of speed based on physical performance limits
        # with a 50% margin to allow the total energy controller to correct for errors

        velRateMax = 0.5 * self._STE_rate_max / self._tas_state
        velRateMin = 0.5 * self._STE_rate_min / self._tas_state

        # 计算限幅后的空速，adj表示限幅
        self._TAS_setpoint_adj = constrain(self._TAS_setpoint, self._TAS_min, self._TAS_max)

        # calculate the demanded rate of change of speed proportional to speed error and apply performance limits
        self._TAS_rate_setpoint = constrain((self._TAS_setpoint_adj - self._tas_state) * self._speed_error_gain, velRateMin, velRateMax)

    def __update_height_setpoint(self, desired:float, state:float):
        # Calculate height error
        # Detect first time through and initialize previous value to demand
        if ISFINITE(desired) and math.fabs(self._hgt_setpoint_in_prev < 0.1):
            self._hgt_setpoint_in_prev = desired

        # Apply a 2 point moving average to demanded height to reduce intersampling noise effects
        if ISFINITE(desired):
            self._hgt_setpoint = 0.5 * (desired + self._hgt_setpoint_in_prev)
        else:
            self._hgt_setpoint = self._hgt_setpoint_in_prev

        self._hgt_setpoint_in_prev = self._hgt_setpoint

        # Apply a rate limit to respect vehicle performance limitations
        if ((self._hgt_setpoint - self._hgt_setpoint_prev) > (self._max_climb_rate * self._dt)):
            self._hgt_setpoint = self._hgt_setpoint_prev + self._max_climb_rate * self._dt
        elif ((self._hgt_setpoint - self._hgt_setpoint_prev) < (-self._max_sink_rate * self._dt)):
            self._hgt_setpoint = self._hgt_setpoint_prev - self._max_sink_rate * self._dt
        self._hgt_setpoint_prev = self._hgt_setpoint

        # Apply a first order noise filter
        self._hgt_setpoint_adj = 0.1 * self._hgt_setpoint + 0.9 * self._hgt_setpoint_adj

        # Calculate the demanded climb rate proportional to height error plus a feedforwward term to
        # provide a tight tracking during steady climb and descent manoeuvres
        self._hgt_rate_setpoint = (
            self._hgt_setpoint_adj - state
        ) * self._height_error_gain + self._height_setpoint_gain_ff * (
            self._hgt_setpoint_adj - self._hgt_setpoint_adj_prev
        ) / self._dt

        self._hgt_setpoint_adj_prev = self._hgt_setpoint_adj

        # Limit the rate of change of height demand to respect vehicle performance limits
        if self._hgt_rate_setpoint > self._max_climb_rate:
            self._hgt_rate_setpoint = self._max_climb_rate
        elif self._hgt_rate_setpoint < -self._max_sink_rate:
            self._hgt_rate_setpoint = -self._max_sink_rate 

    def __detect_underspeed(self):
        if not (self._detect_underspeed_enabled):
            self._underspeed_detected = False
            return
        if (
            (self._tas_state < self._TAS_min * 0.9)
            and (self._throttle_setpoint >= self._throttle_setpoint_max * 0.95)
        ) or (
            (self._vert_pos_state < self._hgt_setpoint_adj)
            and self._underspeed_detected
        ):
            self._underspeed_detected = True
        else:
            self._underspeed_detected = False

    def __update_energy_estimates(self):
        # Calculate specific energy demands in units of (m**2/sec**2)
        self._SPE_setpoint = self._hgt_setpoint_adj * GRAVITY   # potential energy
        self._SKE_setpoint = 0.5 * self._TAS_setpoint_adj ** 2  # kinetic energy

        # Calculate specific energy rate demands in units of (m**2/sec**3)
        self._SPE_rate_setpoint = self._hgt_rate_setpoint * GRAVITY         # potential energy rate of change
        self._SKE_rate_setpoint = self._tas_state * self._TAS_rate_setpoint # kinetic energy rate of change

        # Calculate specific energies in units of (m**2/sec**2)
        self._SPE_estimate = self._vert_pos_state * GRAVITY # potential energy
        self._SKE_estimate = 0.5 * self._tas_state ** 2     # kinetic energy

        # Calculate specific energy rate in units of (m**2/sec**3)
        self._SPE_rate = self._vert_vel_state * GRAVITY             # potential energy rate of change
        self._SKE_rate = self._tas_state * self._speed_derivative  # kinetic energy rate of change

    def __update_throttle_setpoint(self, throttle_cruise:float, rotMat: List[List[float]]):
        # Calculate total energy error
        self._STE_error = self._SPE_setpoint - self._SPE_estimate + self._SKE_setpoint - self._SKE_estimate

        # Calculate demanded rate of change of total energy, respecting vehicle limits
        STE_rate_setpoint = constrain((self._SPE_rate_setpoint + self._SKE_rate_setpoint), self._STE_rate_min, self._STE_rate_max)

        # Calculate the total energy rate error, applying a first order IIR filter
        # to reduce the effect of acceleration noise
        self._STE_rate_error = 0.2 * (STE_rate_setpoint - self._SPE_rate - self._SKE_rate) + 0.8 * self._STE_rate_error

        # Calculate demanded throttle
        if self._underspeed_detected:
            # Always use full throttle to recover from an underspeed condition
            self._throttle_setpoint = 1.0
            rospy.loginfo("Underspeed detected, _throttle_setpoint = 1.0")
        else:
            # Adjust the demanded total energy rate to compensate for induced drag rise in turns.
            # Assume induced drag scales linearly with normal load factor.
            # The additional normal load factor is given by (1/cos(bank angle) - 1)
            cosPhi = math.sqrt((rotMat[0][1] ** 2) + (rotMat[1][1] ** 2))
            STE_rate_setpoint = STE_rate_setpoint + self._load_factor_correction * (1.0 / constrain(cosPhi, 0.1, 1.0) - 1)

            # Calculate a predicted throttle from the demanded rate of change of energy, using the curise throttle as the starting point:
            # Assume:
            # 1. Specific totel energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
            # 2. Specific totel energy rate = 0 at cruise throttle
            # 3. Specific totel energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
            throttle_predicted = 0.0

            # 计算前馈
            if STE_rate_setpoint > 0: 
                # throttle is between cruise and maximum
                throttle_predicted = (
                    throttle_cruise
                    + STE_rate_setpoint
                    / self._STE_rate_max
                    * (self._throttle_setpoint_max - throttle_cruise)
                )
            else:
                # throttle is between cruise and minimum
                throttle_predicted = (
                    throttle_cruise
                    + STE_rate_setpoint
                    / self._STE_rate_min
                    * (self._throttle_setpoint_min - throttle_cruise)
                )

            # Calculate gain scaler from specific energy error to throttle
            STE_to_throttle = 1.0 / (self._throttle_time_constant * (self._STE_rate_max - self._STE_rate_min))

            # Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
            self._throttle_setpoint = (
                self._STE_error + self._STE_rate_error * self._throttle_damping_gain
            ) * STE_to_throttle + throttle_predicted
            self._throttle_setpoint = constrain(
                self._throttle_setpoint,
                self._throttle_setpoint_min,
                self._throttle_setpoint_max,
            )

            # Rate limit the throttle demand
            if (abs(self._throttle_slewrate) > 0.0):
                throttle_increment_limit = (
                    self._dt
                    * (self._throttle_setpoint_max - self._throttle_setpoint_min)
                    * self._throttle_slewrate
                )
                self._throttle_setpoint = constrain(
                    self._throttle_setpoint,
                    self._last_throttle_setpoint - throttle_increment_limit,
                    self._last_throttle_setpoint + throttle_increment_limit,
                )

            self._last_throttle_setpoint = self._throttle_setpoint

            if self._integrator_gain > 0:
                # Calculate throttle integrator state state upper and lower limits with allowance for
                # 10 % throttle saturation to accommodate noise on the demand
                integ_state_max = self._throttle_setpoint_max - self._throttle_setpoint + 0.1
                integ_state_min = self._throttle_setpoint_min - self._throttle_setpoint - 0.1

                # Calculate a throttle demand from the integrated totel energy error
                # This will be added to the total throttle demand to compensate for steady state errors
                self._throttle_integ_state = (
                    self._throttle_integ_state
                    + (self._STE_error * self._integrator_gain)
                    * self._dt
                    * STE_to_throttle
                )

                if self._climbout_mode_active:
                    # During climbout, set the integrator to maximum throttle to prevent transient throttle drop
                    # at the end of climbout when we transition to closed loop throttle control
                    self._throttle_integ_state = integ_state_max
                else:
                    # Respect integrator limits during close loop operation
                    self._throttle_integ_state = constrain(self._throttle_integ_state, integ_state_min, integ_state_max)
            else:
                self._throttle_integ_state = 0.0

            if (self.airspeed_sensor_enabled()):
                # Add the integrator feedback during closed loop operation with an airspeed sensor
                self._throttle_setpoint += self._throttle_integ_state
            else:
                # When flying without an airspeed sensor, use the predicted throttle only
                self._throttle_setpoint = throttle_predicted

            self._throttle_setpoint = constrain(self._throttle_setpoint, self._throttle_setpoint_min, self._throttle_setpoint_max)


    def __detect_uncommanded_descent(self):
        # This function detects a condition that can occur when the demanded airspeed is greater than the
        # aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce height
        # while attempting to maintain speed.

        # Calculate rate of change of total specified energy
        STE_rate = self._SPE_rate + self._SKE_rate

        # If total energy is very low and reducing, throttle is high, and we are not in an underspeed conditon, the enter uncommanded descen recovery mode
        enter_mode = (
            not self._uncommanded_descent_recovery
            and not self._underspeed_detected
            and (self._STE_error > 200.0)
            and (STE_rate < 0.0)
            and (self._throttle_setpoint >= self._throttle_setpoint_max * 0.9)
        )

        # If enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
        exit_mode = (self._uncommanded_descent_recovery and (self._underspeed_detected or self._STE_error < 0.0))

        if enter_mode:
            self._uncommanded_descent_recovery = True
        elif exit_mode:
            self._uncommanded_descent_recovery = False

    def __update_pitch_setpoint(self):
        # The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation.
        # A weighting of 1 givea equal speed and height priority
        # A weighting of 0 gives 100% priority to height control and must be used when no airspeed measurement is available.
        # A weighting of 2 provides 100% priority to speed control and is used when:
        # a) an underspeed condition is detected.
        # b) during climbout where a minimum pitch angle has been set to ensure height is gained. If the airspeed
        # rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
        # The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.

        # Calculate the weighting applied to control of specific kinetic energy error
        SKE_weighting = constrain(self._pitch_speed_weight, 0.0, 2.0)

        if ((self._underspeed_detected or self._climbout_mode_active) and self.airspeed_sensor_enabled()):
            SKE_weighting = 2.0
        elif (not self.airspeed_sensor_enabled()):
            SKE_weighting = 0.0

        # Calculate the weighting applied to control of specific potential energy error
        SPE_weighting = 2.0 - SKE_weighting

        # Calculate the specific energy balance demand which specifies how the available total
        # energy should be allocated between speed and (kinetic energy) and height (potential energy)
        SEB_setpoint = self._SPE_setpoint * SPE_weighting - self._SKE_setpoint * SKE_weighting

        # Calculate the specific energy balance rate demand
        SEB_rate_setpoint = self._SPE_rate_setpoint * SPE_weighting - self._SKE_rate_setpoint * SKE_weighting

        # Calculate the specific energy balacne and balance rate error
        self._SEB_error = SEB_setpoint - (self._SPE_estimate * SPE_weighting - self._SKE_estimate * SKE_weighting)
        self._SEB_rate_error = SEB_rate_setpoint - (self._SPE_rate * SPE_weighting - self._SKE_rate * SKE_weighting)

        # Calculate derivating from change in climb angle to rate of change of specific energy balance
        climb_angle_to_SEB_rate = self._tas_state * self._pitch_time_constant * GRAVITY

        if self._integrator_gain > 0.0:
            # Calculate the pitch integrator input term
            pitch_integ_input = self._SEB_error * self._integrator_gain

            # Prevent the integrator changing in a direction that will increase pitch demand saturation
            # Decay the integrator at the control loop time constant if the pitch demand from the previous time step is staturated
            if self._pitch_setpoint_unc > self._pitch_setpoint_max:
                pitch_integ_input = min(
                    pitch_integ_input,
                    min(
                        (self._pitch_setpoint_max - self._pitch_setpoint_unc)
                        * climb_angle_to_SEB_rate / self._pitch_time_constant,
                        0.0,
                    )
                )
            elif self._pitch_setpoint_unc < self._pitch_setpoint_min:
                pitch_integ_input = max(
                    pitch_integ_input,
                    max(
                        (self._pitch_setpoint_min - self._pitch_setpoint_unc)
                        * climb_angle_to_SEB_rate / self._pitch_time_constant,
                        0.0,
                    )
                )
            
            # Update the pitch integrator state
            self._pitch_integ_state += pitch_integ_input * self._dt

        else:
            self._pitch_integ_state = 0.0

        # Calculate a specific energy correction that doesn't include the integrator contribution
        SEB_correction = self._SEB_error + self._SEB_rate_error * self._pitch_damping_gain + SEB_rate_setpoint * self._pitch_time_constant

        # During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
        # demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
        # having to catch up before the nose can be raised to reduce excess speed during climbout.
        if self._climbout_mode_active:
            SEB_correction += self._pitch_setpoint_min * climb_angle_to_SEB_rate

        # Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
        # a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop
        # b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
        #    pitch transients due to control action or turbulence.
        self._pitch_setpoint_unc = (SEB_correction + self._pitch_integ_state) / climb_angle_to_SEB_rate
        self._pitch_setpoint = constrain(self._pitch_setpoint_unc, self._pitch_setpoint_min, self._pitch_setpoint_max)

        # Comply with the specified vertical acceleration limit by applying a pitch rate limit
        pitchRateIncr = self._dt * self._vert_accel_limit / self._tas_state

        if (self._pitch_setpoint - self._last_pitch_setpoint) > pitchRateIncr:
            self._pitch_setpoint = self._last_pitch_setpoint + pitchRateIncr
        elif (self._pitch_setpoint - self._last_pitch_setpoint) < -pitchRateIncr:
            self._pitch_setpoint = self._last_pitch_setpoint - pitchRateIncr
        
        self._last_pitch_setpoint = self._pitch_setpoint

    def __initialize_states(self, pitch:float, throttle_cruise: float, baro_altitude:float, pitch_min_climbout:float, EAS2TAS:float):
        if (self._pitch_update_timestamp == 0.0 or self._dt > DT_MAX or not self._in_air or not self._states_initialized):
            rospy.loginfo("Initializing states...")
            # On the first time through or when not using TECS of it there has been a large time slip,
            # states must be reset to allow filters to a clean start
            self._vert_accel_state = 0.0
            self._vert_vel_state = 0.0
            self._vert_pos_state = baro_altitude
            self._tas_rate_state = 0.0
            self._tas_state = self._EAS_state * EAS2TAS
            self._throttle_integ_state = 0.0
            self._pitch_integ_state = 0.0
            if self._in_air:
                self._last_throttle_setpoint = throttle_cruise
            else:
                self._last_throttle_setpoint = 0.0
            self._last_pitch_setpoint = constrain(pitch, self._pitch_setpoint_min, self._pitch_setpoint_max)
            self._pitch_setpoint_unc = self._last_pitch_setpoint
            self._hgt_setpoint_adj_prev = baro_altitude
            self._hgt_setpoint_adj = self._hgt_setpoint_adj_prev
            self._hgt_setpoint_prev = self._hgt_setpoint_adj_prev
            self._hgt_setpoint_in_prev = self._hgt_setpoint_adj_prev
            self._TAS_setpoint_last = self._EAS_state * EAS2TAS
            self._TAS_setpoint_adj = self._TAS_setpoint_last
            self._underspeed_detected = False
            self._uncommanded_descent_recovery = False
            self._STE_rate_error = 0.0

            if self._dt > DT_MAX or self._dt < DT_MIN:
                self._dt = DT_DEFAULT
        
        elif self._climbout_mode_active:
            # During climbout use the lower pitch angle limit specified by the calling controller
            self._pitch_setpoint_min = pitch_min_climbout

            # Throttle lower limit is set to a value that prevents throttle reduction
            self._throttle_setpoint_min = self._throttle_setpoint_max - 0.01

            # Height demand and asscociated states are set to track the measured height
            self._hgt_setpoint_adj_prev = baro_altitude
            self._hgt_setpoint_adj = self._hgt_setpoint_adj_prev
            self._hgt_setpoint_prev = self._hgt_setpoint_adj_prev

            # Airspeed demand states are set to track the measured airspeed
            self._TAS_setpoint_last = self._EAS_state * EAS2TAS
            self._TAS_setpoint_adj = self._EAS_state * EAS2TAS

            # Disable speed and decent error condition check
            self._underspeed_detected = False
            self._uncommanded_descent_recovery = False
        
        self._states_initialized = True

    def __update_STE_rate_lim(self):
        # Calculate the specific total energy upper rate limits from the max throttle climb rate
        self._STE_rate_max = self._max_climb_rate * GRAVITY
        
        # Calculate the specific total energy lower rate limits from the min throttle climb rate
        self._STE_rate_min = - self._min_sink_rate * GRAVITY

    # Update the control loop calculations
    def update_pitch_throttle(self, rotMat:List[List[float]], pitch:float, baro_altitude:float, hgt_setpoint:float, EAS_setpoint:float,
                              indicated_airspeed:float, eas_to_tas:float, climb_out_setpoint:bool, pitch_min_climbout:float,
                              throttle_min:float, throttle_max:float, throttle_cruise:float, pitch_limit_min:float, pitch_limit_max:float):
        """
        @Description :
            定义update_pitch_throttle函数，用于更新 pitch 和 throttle
        @Param:
            参数rotMat：旋转矩阵
            参数pitch： pitch
            参数baro_altitude： 海拔高度
            参数hgt_setpoint： 设置高度
            参数EAS_setpoint： 理想空速
            参数indicated_airspeed： 实测空速
            参数eas_to_tas： 实测空速到理想空速的转换系数
            参数climb_out_setpoint： 爬升出厂设置
            参数pitch_min_climbout： 最小爬升pitch
            参数throttle_min： 最小油门
            参数throttle_max： 最大油门
            参数throttle_cruise： 巡航油门
            参数pitch_limit_min： pitch最小限制
            参数pitch_limit_max： pitch最大限制
        @Returns     :
            pitch 和 throttle
        """
        # Calculate the time since last update (seconds)
        now = time.time()
        self._dt = constrain((now - self._pitch_update_timestamp), DT_MIN, DT_MAX)

        # Set class variables from inputs
        self._throttle_setpoint_max = throttle_max
        self._throttle_setpoint_min = throttle_min
        self._pitch_setpoint_max = pitch_limit_max
        self._pitch_setpoint_min = pitch_limit_min
        self._climbout_mode_active = climb_out_setpoint

        # Initialize selected states and variables as required
        # 以下情况，初始化控制器：
        # 1. 初次进入控制器
        # 2. 控制器间隔dt超时
        # 3. 不在空中
        # 4. 调用reset函数后

        self.__initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas)

        # Don't run TECS control algorithms when not in flight
        if not self._in_air:
            return
        
        # Update the true airspeed state estimate
        # 1. 此函数更新飞机的空速，以及空速变化率
        # 2. 将飞机的空速进行二阶低通滤波
        self.__update_speed_state(EAS_setpoint, indicated_airspeed, eas_to_tas)

        # Calculate rate limits for specific total energy
        self.__update_STE_rate_lim()

        # Detect an underspeed condition
        self.__detect_underspeed()

        # Detect an uncommanded descent caused by an unachieveable airspeed demand
        self.__detect_uncommanded_descent()

        # Calculate the commanded true airspeed
        # 计算期望空速以及期望空速变化率（加速度）限幅
        self.__update_speed_setpoint()

        # Calculate the demanded height
	    # 计算期望的高度值以及高度变化率
        self.__update_height_setpoint(hgt_setpoint, baro_altitude)

        # Calculate the specific energy values required by the control loop
        # 计算总能量
        self.__update_energy_estimates()

        # Calculate the throttle demand
        # 更新油门设定值
        self.__update_throttle_setpoint(throttle_cruise, rotMat)

        # Calculate the pitch demand
        self.__update_pitch_setpoint()

        # Update time stamps
        self._pitch_update_timestamp = now

        # Set TECS mode for next frame
        if self._underspeed_detected:
            self._tecs_mode = ECL_TECS_MODE_UNDERSPEED
        elif self._uncommanded_descent_recovery:
            self._tecs_mode = ECL_TECS_MODE_BAD_DESCENT
        elif self._climbout_mode_active:
            self._tecs_mode = ECL_TECS_MODE_CLIMBOUT
        else:
            # Default to normal flight mode
            self._tecs_mode = ECL_TECS_MODE_NORMAL
